/**
    ATClab - scriptable Air Traffic Control simulations.
    Copyright (C) 2010 openatclab@gmail.com

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
**/

#include "aircraft_data.h"
#include "atc.h"
#include <algorithm>

using namespace atc;

////////////////////////////////////////////////////////////////////////////////
//
// atc::AircraftPath
//
//------------------------------------------------------------------------------
// construction/destruction
//

/*!
 *
 */
AircraftPath::AircraftPath() 
	  : _plan()
{}

/*!
 *
 *
 */
AircraftPath::AircraftPath( const AircraftPath &rhs ) 
	  : _plan()
{
	AircraftPath::const_iterator it = rhs.begin();
	for ( ; it != rhs.end(); ++ it )
		push( **it );
}

/*!
 *
 */
AircraftPath::~AircraftPath() {
	AircraftPath::const_iterator it = begin();
	for( ; it != end(); ++it )
		delete *it;
}


//------------------------------------------------------------------------------
// [public] api
//

/*!
 *
 */
void AircraftPath::push( const AircraftPathToken &t ) { 
	_plan.push_back( new AircraftPathToken( t ) ); 
}

/*!
 *
 */
bool AircraftPath::is_valid() const { 
	return !_plan.empty(); 
}


/*!
 *
 */
void AircraftPath::next() {
	//_track.push_back( _plan.back() );
	delete _plan.front();
	_plan.pop_front();
}

/*!
 *
 */
bool AircraftPath::done() { 
	return _plan.empty(); 
}

/*!
 *
 */
const AircraftPathToken* AircraftPath::data() const { 
	return _plan.front(); 
}

/*!
 *
 */
AircraftPath::const_iterator AircraftPath::begin() const { 
	return _plan.begin(); 
}

/*!
 *
 */
AircraftPath::const_iterator AircraftPath::end() const {
	return _plan.end();
}

/*!
 *
 */
Point AircraftPath::future_point( const Point &pt, double s ) const {
	return future_point( begin(), pt, s );
}


/*!
 *
 */
void AircraftPath::reroute( 
		const AircraftPathToken *t, const Point &p1, const Point &p0 
) {
	
	while ( !done() && t != data() )
		next();	
	
	AircraftPathToken *ct = _plan.front();
	ct->x0 = p1.first;
	ct->y0 = p1.second;

	_plan.push_front( new AircraftPathToken (
			p0.first, p0.second, NAN, NAN
		  , p1.first, p1.second, NAN, NAN
	) );
}


//------------------------------------------------------------------------------
// [private] implemetation
//

/*!
 *
 */
Point AircraftPath::future_point( 
		const_iterator ps, const Point &p0, double s 
) const {
	Point p1( (*ps)->x1, (*ps)->y1 );
	
	double dx = p1.first - p0.first;
	double dy = p1.second - p0.second;
	double length = _hypot( dx, dy );

	if ( s > length ) {
		++ps;
		if   ( ps == end() )  return p1;
		else                  return future_point( ps, p1, s - length );
	}
	else {
		double a = atan2( dy, dx );
		return Point( p0.first + s * cos( a ), p0.second + s * sin( a ) );
	}
}


////////////////////////////////////////////////////////////////////////////////
//
// atc::AircraftHistory
//
//------------------------------------------------------------------------------
// 
//

/*!
 *
 */
AircraftHistory::AircraftHistory() : _history() {}

/*!
 *
 */
AircraftHistory::~AircraftHistory() {
	std::deque< const AircraftState * >::iterator it = _history.begin();
	for ( ; it != _history.end(); ++it ) {
		delete *it;
	}
}


//------------------------------------------------------------------------------
// 
//

/*!
 * Add Aircraft State to the History Log.
 *
 * State information is added to the top of the history log. Once the log 
 * reaches a certain size it is maintained at that size with each additional
 * history entry causing the earliest entry to be dropped off the bottom.
 */
void AircraftHistory::add_event( const AircraftState *state ) {
	_history.push_front( state );
//	if ( _history.size() > HISTORY_DOT_COUNT ) {
//		_history.pop_back(); // @todo: does this leak?
//	}
}


////////////////////////////////////////////////////////////////////////////////
